#include <stdint.h>
#include <stdbool.h>
#include "inc/hw_ints.h"
#include "inc/hw_memmap.h"
#include "driverlib/debug.h"
#include "driverlib/fpu.h"
#include "driverlib/gpio.h"
#include "driverlib/interrupt.h"
#include "driverlib/pin_map.h"
#include "driverlib/rom.h"
#include "driverlib/sysctl.h"
#include "driverlib/uart.h"
#include "driverlib/pwm.h"
#include "motor-init.h"

#define IN1B 	GPIO_PIN_6			//PB6
#define IN2B 	GPIO_PIN_7			//PB7
#define IN1A 	GPIO_PIN_3			//PA3
#define IN2A 	GPIO_PIN_2			//PA2

int power[4];
// [pwmdx][pwmsx][ulPeriod][100]
// [delta1][delta2]


//*****************************************************************************
//
// Funzione per mandare avanti i motori
//
//*****************************************************************************
int ulPeriod;

//void pwm_power(int delta_1, int delta_2)
void pwm_power(int delta_1,int delta_2)
{

		power[0]= delta_1;
		power[1]= delta_2;

		//delta_1=(ulPeriod*delta_1/100);
		//delta_2=(ulPeriod*delta_2/100);
		power[0]=(power[2]*power[0]/power[3]);
		power[1]=(power[2]*power[1]/power[3]);
	    PWMPulseWidthSet(PWM0_BASE, PWM_OUT_3,power[0]);
	    PWMPulseWidthSet(PWM0_BASE, PWM_OUT_2,power[1]);

}

void pwm_dir(int dir_1, int dir_2)
{
	switch(dir_1)
	{
		case(10):
			{
				GPIOPinWrite(GPIO_PORTA_BASE, IN1A | IN2A, IN1A);
				break;
			}

		case(20):
			{
				GPIOPinWrite(GPIO_PORTA_BASE, IN1A | IN2A, IN2A);
				break;
			}

			case(30):

			{
				GPIOPinWrite(GPIO_PORTA_BASE, IN1A | IN2A, 0);
				break;
			}

		default:
			{
				GPIOPinWrite(GPIO_PORTA_BASE, IN1A | IN2A, 0);
				break;
			}
	}

	switch(dir_2)
	{
		case(10):
			{
				GPIOPinWrite(GPIO_PORTB_BASE, IN1B | IN2B, IN1B);
				break;
			}

		case(20):
			{
				GPIOPinWrite(GPIO_PORTB_BASE, IN1B | IN2B, IN2B);
				break;
			}

			case(30):

			{
				GPIOPinWrite(GPIO_PORTB_BASE, IN1B | IN2B, 0);
				break;
			}

		default:
			{
				GPIOPinWrite(GPIO_PORTB_BASE, IN1B | IN2B, 0);
				break;
			}
	}




}



void int_init(void)
{
    //
    // Enable lazy stacking for interrupt handlers.  This allows floating-point
    // instructions to be used within interrupt handlers, but at the expense of
    // extra stack usage.
    //
    ROM_FPUEnable();
    ROM_FPULazyStackingEnable();

    //
    // Enable processor interrupts.
    //
    IntMasterEnable();

    //
    // Enable the UART interrupt.
    //
    IntEnable(INT_UART0);
    IntEnable(INT_UART1);
    UARTIntEnable(UART0_BASE, UART_INT_RX | UART_INT_RT);
    UARTIntEnable(UART1_BASE, UART_INT_RX | UART_INT_RT);


}



void pwm_init(void)
{

	power[2]= ulPeriod = SysCtlClockGet() / 16000;
	power[3]= 100;

	ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
	ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);

	GPIOPinTypeGPIOOutput(GPIO_PORTB_BASE, IN1A | IN2A);
	GPIOPinTypeGPIOOutput(GPIO_PORTA_BASE, IN1B | IN2B);




    //
    // Enable the GPIO port B
    //
	ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
	ROM_GPIOPinTypeGPIOOutput(GPIO_PORTB_BASE, GPIO_PIN_4 | GPIO_PIN_5);


	int ulPeriod;
    ulPeriod = SysCtlClockGet() / 16000; //PWM frequency 400HZ
    ROM_SysCtlPWMClockSet(SYSCTL_PWMDIV_2);  //divisore per 2
    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM0); //modulo pwm numero 0

    //Configure PB4, PB5 Pins as PWM
    ROM_GPIOPinConfigure(GPIO_PB5_M0PWM3);  //generatore 3
    ROM_GPIOPinConfigure(GPIO_PB4_M0PWM2); // generatore 4
    ROM_GPIOPinTypePWM(GPIO_PORTB_BASE, GPIO_PIN_4 | GPIO_PIN_5);


    //il generatore dovrebbe essere il numero 1
    //TODO: verificare che sia vero
    ROM_PWMGenConfigure(PWM0_BASE, PWM_GEN_1, PWM_GEN_MODE_UP_DOWN | PWM_GEN_MODE_NO_SYNC);


    //Set the Period (expressed in clock ticks)
    ROM_PWMGenPeriodSet(PWM0_BASE, PWM_GEN_1, ulPeriod);


    //Set PWM duty-50% (Period /2)
    ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_2,0);
    ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_3,0);




    // Enable the PWM generator
    ROM_PWMGenEnable(PWM0_BASE, PWM_GEN_1);

    // Turn on the Output pins
    ROM_PWMOutputState(PWM0_BASE, PWM_OUT_3_BIT|PWM_OUT_2_BIT, true);

}

void console_init(void)
{
    //
    // Enable the peripherals used by this example.
    //
	ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_UART0);
	ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);

	ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_UART1);
	ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);

    //
    // Set GPIO A0 and A1 as UART pins.
    //
	ROM_GPIOPinConfigure(GPIO_PA0_U0RX);
	ROM_GPIOPinConfigure(GPIO_PA1_U0TX);
	ROM_GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);

	ROM_GPIOPinConfigure(GPIO_PB0_U1RX);
	ROM_GPIOPinConfigure(GPIO_PB1_U1TX);
	ROM_GPIOPinTypeUART(GPIO_PORTB_BASE, GPIO_PIN_0 | GPIO_PIN_1);

	//
    // Configure the UART for 115,200, 8-N-1 operation.
    //
	ROM_UARTConfigSetExpClk(UART0_BASE, ROM_SysCtlClockGet(), 115200,
                            (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE |
                             UART_CONFIG_PAR_NONE));

	ROM_UARTConfigSetExpClk(UART1_BASE, ROM_SysCtlClockGet(), 115200,
                            (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE |
                             UART_CONFIG_PAR_NONE));
}
